#include "satellite.h"

ErrorCode satellite_init(Satellite* sat, const char* name) {
    if (!sat || !name) return ERROR_INVALID_INPUT;
    
    strncpy(sat->name, name, sizeof(sat->name) - 1);
    sat->name[sizeof(sat->name) - 1] = '\0';
    
    // 初始化轨道参数
    sat->position = vector3d_create(7000000.0, 0.0, 0.0);  // 7000km高度
    sat->velocity = vector3d_create(0.0, 7500.0, 0.0);     // 轨道速度
    sat->mass = SATELLITE_MASS;
    
    // 初始化姿态
    sat->attitude = quaternion_identity();
    sat->angular_velocity = vector3d_zero();
    sat->angular_acceleration = vector3d_zero();
    
    // 初始化惯性张量 (简化模型)
    for (int i = 0; i < 3; i++) {
        for (int j = 0; j < 3; j++) {
            sat->inertia_tensor[i][j] = (i == j) ? 100.0 : 0.0;
            sat->inertia_inverse[i][j] = (i == j) ? 0.01 : 0.0;
        }
    }
    
    // 初始化子系统
    attitude_controller_init(&sat->attitude_controller);
    
    // 初始化离子发动机
    ThrusterSpec thruster_spec = {
        .max_thrust = MAX_THRUST,
        .min_thrust = 0.001,
        .specific_impulse = ISP,
        .power_consumption = 1000.0,
        .warmup_time = 30.0,
        .response_time = 5.0,
        .efficiency = 0.8
    };
    
    for (int i = 0; i < 4; i++) {
        ion_thruster_init(&sat->thruster[i], &thruster_spec);
    }
    
    // 初始化GNSS接收机
    gnss_simulate_data(NULL, NULL, &sat->gnss_receiver);
    
    sat->timestamp = 0.0;
    sat->is_active = true;
    
    return SUCCESS;
}

ErrorCode satellite_update(Satellite* sat, double dt) {
    if (!sat || dt <= 0.0) return ERROR_INVALID_INPUT;
    
    sat->timestamp += dt;
    
    // 更新轨道动力学
    satellite_calculate_dynamics(sat, dt);
    
    // 更新姿态控制器
    sat->attitude_controller.current_attitude = sat->attitude;
    sat->attitude_controller.current_angular_velocity = sat->angular_velocity;
    attitude_controller_update(&sat->attitude_controller, dt);
    
    // 获取控制力矩
    double torque[3];
    attitude_controller_get_torque(&sat->attitude_controller, torque);
    
    // 应用控制力矩
    Vector3D control_torque = vector3d_create(torque[0], torque[1], torque[2]);
    satellite_apply_torque(sat, control_torque, dt);
    
    // 更新离子发动机
    for (int i = 0; i < 4; i++) {
        ion_thruster_update(&sat->thruster[i], dt);
    }
    
    // 更新GNSS
    satellite_update_gnss(sat);
    
    return SUCCESS;
}

ErrorCode satellite_calculate_dynamics(Satellite* sat, double dt) {
    if (!sat || dt <= 0.0) return ERROR_INVALID_INPUT;
    
    // 计算重力
    double r = vector3d_magnitude(sat->position);
    double r3 = r * r * r;
    Vector3D gravity = vector3d_scale(sat->position, -MU / r3);
    
    // 应用重力
    satellite_apply_force(sat, gravity, dt);
    
    // 姿态动力学
    Vector3D angular_acceleration = vector3d_zero();
    
    // 计算角加速度 (I * α = τ)
    for (int i = 0; i < 3; i++) {
        for (int j = 0; j < 3; j++) {
            angular_acceleration.v[i] += sat->inertia_inverse[i][j] * sat->angular_acceleration.v[j];
        }
    }
    
    sat->angular_velocity = vector3d_add(sat->angular_velocity, 
                                       vector3d_scale(angular_acceleration, dt));
    
    // 更新姿态四元数
    Vector3D omega = sat->angular_velocity;
    double omega_mag = vector3d_magnitude(omega);
    
    if (omega_mag > 1e-6) {
        Vector3D axis = vector3d_normalize(omega);
        double angle = omega_mag * dt;
        Quaternion delta_q = quaternion_from_axis_angle(axis, angle);
        sat->attitude = quaternion_multiply(sat->attitude, delta_q);
        sat->attitude = quaternion_normalize(sat->attitude);
    }
    
    return SUCCESS;
}

ErrorCode satellite_apply_torque(Satellite* sat, Vector3D torque, double dt) {
    if (!sat || dt <= 0.0) return ERROR_INVALID_INPUT;
    
    sat->angular_acceleration = torque;  // 简化模型
    
    return SUCCESS;
}

ErrorCode satellite_apply_force(Satellite* sat, Vector3D force, double dt) {
    if (!sat || dt <= 0.0) return ERROR_INVALID_INPUT;
    
    Vector3D acceleration = vector3d_scale(force, 1.0 / sat->mass);
    sat->velocity = vector3d_add(sat->velocity, vector3d_scale(acceleration, dt));
    sat->position = vector3d_add(sat->position, vector3d_scale(sat->velocity, dt));
    
    return SUCCESS;
}

ErrorCode satellite_set_target_attitude(Satellite* sat, Quaternion target) {
    if (!sat) return ERROR_INVALID_INPUT;
    
    return attitude_controller_set_target(&sat->attitude_controller, target);
}

ErrorCode satellite_point_to_direction(Satellite* sat, Vector3D direction) {
    if (!sat) return ERROR_INVALID_INPUT;
    
    Vector3D target = vector3d_normalize(direction);
    Vector3D current = vector3d_create(1.0, 0.0, 0.0);  // 卫星+X轴
    
    Quaternion target_attitude = quaternion_from_to_rotation(current, target);
    return satellite_set_target_attitude(sat, target_attitude);
}

ErrorCode satellite_stabilize(Satellite* sat) {
    if (!sat) return ERROR_INVALID_INPUT;
    
    sat->attitude_controller.target_angular_velocity = vector3d_zero();
    return attitude_controller_set_mode(&sat->attitude_controller, ATTITUDE_MODE_STABILIZE);
}

ErrorCode satellite_slew_maneuver(Satellite* sat, Quaternion target, double duration) {
    if (!sat || duration <= 0.0) return ERROR_INVALID_INPUT;
    
    // 计算最优旋转路径
    Quaternion optimal = attitude_optimal_rotation(sat->attitude, target);
    
    // 设置目标姿态
    return satellite_set_target_attitude(sat, target);
}

ErrorCode satellite_update_gnss(Satellite* sat) {
    if (!sat) return ERROR_INVALID_INPUT;
    
    GNSS_Satellite satellites[MAX_SATELLITES];
    int count;
    
    gnss_simulate_data(satellites, &count, &sat->gnss_receiver);
    
    return SUCCESS;
}

ErrorCode satellite_update_sensors(Satellite* sat) {
    if (!sat) return ERROR_INVALID_INPUT;
    
    // 更新各种传感器数据
    satellite_update_gnss(sat);
    
    return SUCCESS;
}

void satellite_print_status(Satellite* sat) {
    if (!sat) return;
    
    printf("=== 卫星状态: %s ===\n", sat->name);
    printf("时间: %.1f s\n", sat->timestamp);
    
    satellite_print_orbit(sat);
    satellite_print_attitude(sat);
    
    printf("质量: %.1f kg\n", sat->mass);
    printf("状态: %s\n", sat->is_active ? "活跃" : "非活跃");
}

void satellite_print_orbit(Satellite* sat) {
    if (!sat) return;
    
    printf("--- 轨道参数 ---\n");
    printf("位置 (ECEF): ");
    vector3d_print("", sat->position);
    
    Vector3D geodetic = gnss_ecef_to_geodetic(sat->position);
    printf("位置 (LLA): [%.6f°, %.6f°, %.1f km]\n", 
           geodetic.x, geodetic.y, geodetic.z / 1000.0);
    
    printf("速度: ");
    vector3d_print("", sat->velocity);
    
    double altitude = vector3d_magnitude(sat->position) - EARTH_RADIUS;
    printf("轨道高度: %.1f km\n", altitude / 1000.0);
    
    double speed = vector3d_magnitude(sat->velocity);
    printf("轨道速度: %.1f m/s\n", speed);
}

void satellite_print_attitude(Satellite* sat) {
    if (!sat) return;
    
    printf("--- 姿态参数 ---\n");
    Vector3D euler = quaternion_to_euler(sat->attitude);
    printf("姿态 (欧拉角): [%.2f°, %.2f°, %.2f°]\n", 
           euler.x * RAD_TO_DEG, 
           euler.y * RAD_TO_DEG, 
           euler.z * RAD_TO_DEG);
    
    printf("角速度: ");
    vector3d_print("", sat->angular_velocity);
    
    attitude_print_status(&sat->attitude_controller);
}

ErrorCode satellite_get_state_string(Satellite* sat, char* buffer, size_t size) {
    if (!sat || !buffer || size == 0) return ERROR_INVALID_INPUT;
    
    Vector3D geodetic = gnss_ecef_to_geodetic(sat->position);
    Vector3D euler = quaternion_to_euler(sat->attitude);
    
    snprintf(buffer, size,
        "Satellite: %s\n"
        "Time: %.1f s\n"
        "Position: %.1f, %.1f, %.1f km\n"
        "Velocity: %.1f m/s\n"
        "Attitude: %.1f, %.1f, %.1f deg\n"
        "Fuel: %.1f kg",
        sat->name,
        sat->timestamp,
        geodetic.x, geodetic.y, geodetic.z / 1000.0,
        vector3d_magnitude(sat->velocity),
        euler.x * RAD_TO_DEG, euler.y * RAD_TO_DEG, euler.z * RAD_TO_DEG,
        sat->thruster[0].fuel_remaining
    );
    
    return SUCCESS;
}
